

#include "arm_math.h"
#include "arm_const_structs.h"
#include "dsp/support_functions.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

const q15_t __ALIGNED(4) RealCoefAQ15[128] ARM_DSP_TABLE_ATTRIBUTE = {
    (q15_t)0x4000, (q15_t)0xc000, (q15_t)0x3cdc, (q15_t)0xc014, (q15_t)0x39ba, (q15_t)0xc04f, (q15_t)0x369c, (q15_t)0xc0b1,
    (q15_t)0x3384, (q15_t)0xc13b, (q15_t)0x3073, (q15_t)0xc1eb, (q15_t)0x2d6c, (q15_t)0xc2c1, (q15_t)0x2a70, (q15_t)0xc3be,
    (q15_t)0x2782, (q15_t)0xc4df, (q15_t)0x24a3, (q15_t)0xc625, (q15_t)0x21d5, (q15_t)0xc78f, (q15_t)0x1f19, (q15_t)0xc91b,
    (q15_t)0x1c72, (q15_t)0xcac9, (q15_t)0x19e0, (q15_t)0xcc98, (q15_t)0x1766, (q15_t)0xce87, (q15_t)0x1505, (q15_t)0xd094,
    (q15_t)0x12bf, (q15_t)0xd2bf, (q15_t)0x1094, (q15_t)0xd505, (q15_t)0x0e87, (q15_t)0xd766, (q15_t)0x0c98, (q15_t)0xd9e0,
    (q15_t)0x0ac9, (q15_t)0xdc72, (q15_t)0x091b, (q15_t)0xdf19, (q15_t)0x078f, (q15_t)0xe1d5, (q15_t)0x0625, (q15_t)0xe4a3,
    (q15_t)0x04df, (q15_t)0xe782, (q15_t)0x03be, (q15_t)0xea70, (q15_t)0x02c1, (q15_t)0xed6c, (q15_t)0x01eb, (q15_t)0xf073,
    (q15_t)0x013b, (q15_t)0xf384, (q15_t)0x00b1, (q15_t)0xf69c, (q15_t)0x004f, (q15_t)0xf9ba, (q15_t)0x0014, (q15_t)0xfcdc,
    (q15_t)0x0000, (q15_t)0x0000, (q15_t)0x0014, (q15_t)0x0324, (q15_t)0x004f, (q15_t)0x0646, (q15_t)0x00b1, (q15_t)0x0964,
    (q15_t)0x013b, (q15_t)0x0c7c, (q15_t)0x01eb, (q15_t)0x0f8d, (q15_t)0x02c1, (q15_t)0x1294, (q15_t)0x03be, (q15_t)0x1590,
    (q15_t)0x04df, (q15_t)0x187e, (q15_t)0x0625, (q15_t)0x1b5d, (q15_t)0x078f, (q15_t)0x1e2b, (q15_t)0x091b, (q15_t)0x20e7,
    (q15_t)0x0ac9, (q15_t)0x238e, (q15_t)0x0c98, (q15_t)0x2620, (q15_t)0x0e87, (q15_t)0x289a, (q15_t)0x1094, (q15_t)0x2afb,
    (q15_t)0x12bf, (q15_t)0x2d41, (q15_t)0x1505, (q15_t)0x2f6c, (q15_t)0x1766, (q15_t)0x3179, (q15_t)0x19e0, (q15_t)0x3368,
    (q15_t)0x1c72, (q15_t)0x3537, (q15_t)0x1f19, (q15_t)0x36e5, (q15_t)0x21d5, (q15_t)0x3871, (q15_t)0x24a3, (q15_t)0x39db,
    (q15_t)0x2782, (q15_t)0x3b21, (q15_t)0x2a70, (q15_t)0x3c42, (q15_t)0x2d6c, (q15_t)0x3d3f, (q15_t)0x3073, (q15_t)0x3e15,
    (q15_t)0x3384, (q15_t)0x3ec5, (q15_t)0x369c, (q15_t)0x3f4f, (q15_t)0x39ba, (q15_t)0x3fb1, (q15_t)0x3cdc, (q15_t)0x3fec,
};
const q15_t __ALIGNED(4) RealCoefBQ15[128] ARM_DSP_TABLE_ATTRIBUTE = {
    (q15_t)0x4000, (q15_t)0x4000, (q15_t)0x4324, (q15_t)0x3fec, (q15_t)0x4646, (q15_t)0x3fb1, (q15_t)0x4964, (q15_t)0x3f4f,
    (q15_t)0x4c7c, (q15_t)0x3ec5, (q15_t)0x4f8d, (q15_t)0x3e15, (q15_t)0x5294, (q15_t)0x3d3f, (q15_t)0x5590, (q15_t)0x3c42,
    (q15_t)0x587e, (q15_t)0x3b21, (q15_t)0x5b5d, (q15_t)0x39db, (q15_t)0x5e2b, (q15_t)0x3871, (q15_t)0x60e7, (q15_t)0x36e5,
    (q15_t)0x638e, (q15_t)0x3537, (q15_t)0x6620, (q15_t)0x3368, (q15_t)0x689a, (q15_t)0x3179, (q15_t)0x6afb, (q15_t)0x2f6c,
    (q15_t)0x6d41, (q15_t)0x2d41, (q15_t)0x6f6c, (q15_t)0x2afb, (q15_t)0x7179, (q15_t)0x289a, (q15_t)0x7368, (q15_t)0x2620,
    (q15_t)0x7537, (q15_t)0x238e, (q15_t)0x76e5, (q15_t)0x20e7, (q15_t)0x7871, (q15_t)0x1e2b, (q15_t)0x79db, (q15_t)0x1b5d,
    (q15_t)0x7b21, (q15_t)0x187e, (q15_t)0x7c42, (q15_t)0x1590, (q15_t)0x7d3f, (q15_t)0x1294, (q15_t)0x7e15, (q15_t)0x0f8d,
    (q15_t)0x7ec5, (q15_t)0x0c7c, (q15_t)0x7f4f, (q15_t)0x0964, (q15_t)0x7fb1, (q15_t)0x0646, (q15_t)0x7fec, (q15_t)0x0324,
    (q15_t)0x8000, (q15_t)0x0000, (q15_t)0x7fec, (q15_t)0xfcdc, (q15_t)0x7fb1, (q15_t)0xf9ba, (q15_t)0x7f4f, (q15_t)0xf69c,
    (q15_t)0x7ec5, (q15_t)0xf384, (q15_t)0x7e15, (q15_t)0xf073, (q15_t)0x7d3f, (q15_t)0xed6c, (q15_t)0x7c42, (q15_t)0xea70,
    (q15_t)0x7b21, (q15_t)0xe782, (q15_t)0x79db, (q15_t)0xe4a3, (q15_t)0x7871, (q15_t)0xe1d5, (q15_t)0x76e5, (q15_t)0xdf19,
    (q15_t)0x7537, (q15_t)0xdc72, (q15_t)0x7368, (q15_t)0xd9e0, (q15_t)0x7179, (q15_t)0xd766, (q15_t)0x6f6c, (q15_t)0xd505,
    (q15_t)0x6d41, (q15_t)0xd2bf, (q15_t)0x6afb, (q15_t)0xd094, (q15_t)0x689a, (q15_t)0xce87, (q15_t)0x6620, (q15_t)0xcc98,
    (q15_t)0x638e, (q15_t)0xcac9, (q15_t)0x60e7, (q15_t)0xc91b, (q15_t)0x5e2b, (q15_t)0xc78f, (q15_t)0x5b5d, (q15_t)0xc625,
    (q15_t)0x587e, (q15_t)0xc4df, (q15_t)0x5590, (q15_t)0xc3be, (q15_t)0x5294, (q15_t)0xc2c1, (q15_t)0x4f8d, (q15_t)0xc1eb,
    (q15_t)0x4c7c, (q15_t)0xc13b, (q15_t)0x4964, (q15_t)0xc0b1, (q15_t)0x4646, (q15_t)0xc04f, (q15_t)0x4324, (q15_t)0xc014,
};


#define NUM 128

int16_t double2q15(double value)
{
    return value * 32768 + (value > 0.0 ? 0.5 : -0.5);
}
double q15ToDouble(q15_t value, int pointNum)
{
    double base = 0x1 << pointNum;
    return value / base;
}
void print_q15_array(const char *title, q15_t data[], int len)
{
    printf("%s[%d]:\n", title, len);
    int n = 0;
    while (len-- > 0) {
        printf("%d, ", *data++);
        if (++n >= 16) {
            n = 0;
            printf("\n");
        }
    }
    if (n % 16 != 0) {
        printf("\n");
    }
}

void print_q15_array_as_double(const char *title, q15_t data[], int len, int pointNum)
{
    printf("%s[%d]:\n", title, len);
    int n = 0;
    while (len-- > 0) {
        printf("%lf, ", q15ToDouble(*data++, pointNum));
        if (++n >= 16) {
            n = 0;
            printf("\n");
        }
    }
    if (n % 16 != 0) {
        printf("\n");
    }
}
int main(void)
{
    q15_t src[NUM];
    for (int i = 0; i < sizeof(src) / sizeof(src[0]); ++i) {
        // Fsample = 128k
        // Fsourse = 1k

        src[i] = double2q15(cos(2 * 2 * 3.1415926 * i / (NUM)));
    }
    // print_q15_array_as_double("src", src, NUM, 15);
    q15_t dst[NUM];

    arm_rfft_instance_q15 rfft = {

        /*  Initialize the Real FFT length */
        .fftLenReal = (uint16_t)NUM,

        /*  Initialize the Twiddle coefficientA pointer */
        .pTwiddleAReal = (q15_t *)RealCoefAQ15,

        /*  Initialize the Twiddle coefficientB pointer */
        .pTwiddleBReal = (q15_t *)RealCoefBQ15,

        /*  Initialize the Flag for selection of RFFT or RIFFT */
        .ifftFlagR = (uint8_t)0,

        /*  Initialize the Flag for calculation Bit reversal or not */
        .bitReverseFlagR = (uint8_t)1,

        .twidCoefRModifier = 1,

        .pCfft = &arm_cfft_sR_q15_len64,
    };
    arm_status ret = arm_rfft_init_128_q15(&rfft, 0, 1);
    printf("arm_rfft_init_128_q15: %d\n", ret);

    // rfft.pTwiddleAReal = (q15_t *)RealCoefAQ15;
    // rfft.pTwiddleBReal = (q15_t *)RealCoefBQ15;
    // rfft.twidCoefRModifier = 1;
    arm_rfft_q15(&rfft, src, dst);

    print_q15_array_as_double("result", dst, NUM, 9);

    printf("finished!\n");
    exit(0);
    return 0;
}
